//
//  ControleMotores.h
//  UGVBR
//
//  Created by Felipe Barreiros on 6/26/12.
//  Copyright (c) 2012 __MyCompanyName__. All rights reserved.
//

// ==============================================================================
// Controle de Motores

/*
 int HabilitaMotores = 22;
 
 int Motor0FW 24;
 int Motor0RW 26;
 
 int Motor1FW 28;
 int Motor1RW 30;
 */


#define potenciaMotores 2

#define Motor0FW 24
#define Motor0RW 26

#define Motor1FW 28
#define Motor1RW 30


// Valores para motor:
//      0 Motor0
//      1 Motor1
//      2 Motores 0 e 1
//
// Valores para sentido: 
//      0 FW
//      1 RW
// Valores para Potencia:
//      0 a 255 (saída PWM)
//
void controlaMotor(byte motor, byte sentido, byte potencia){
    
    if(motor == 0){
        // Pra frente
        if(sentido == 0){
            digitalWrite(Motor0RW, LOW);
            digitalWrite(Motor0FW, HIGH);
        }
        // Pra tras
        if(sentido == 1) {
            digitalWrite(Motor0RW, HIGH);
            digitalWrite(Motor0FW, LOW);
        }
    }
    
    if(motor == 1){
        // Pra frente
        if(sentido == 0){
            digitalWrite(Motor1RW, LOW);
            digitalWrite(Motor1FW, HIGH);
        }
        // Pra tras
        if(sentido == 1) {
            digitalWrite(Motor1RW, HIGH);
            digitalWrite(Motor1FW, LOW);
        }
    }
    
    analogWrite(potenciaMotores, potencia);
    
}

// Valores para motor:
//      0 Motor0
//      1 Motor1
//      2 Motores 0 e 1
//
void desligaMotor(byte motor){
    if(motor == 0){
        digitalWrite(Motor0FW, LOW);
        digitalWrite(Motor0RW, LOW);
    }
    else if(motor == 1){
        digitalWrite(Motor1FW, LOW);
        digitalWrite(Motor1RW, LOW);
    }
    else if(motor == 2){
        digitalWrite(Motor0FW, LOW);
        digitalWrite(Motor0RW, LOW);
        digitalWrite(Motor1FW, LOW);
        digitalWrite(Motor1RW, LOW);
    }
}

// Valores para motor:
//      0 Motor0
//      1 Motor1
//      2 Motores 0 e 1
// Valores para tempo:
//      Qualquer valor inteiro em ms
//
void travaMotor(byte motor){
    if(motor == 0){
        digitalWrite(Motor0FW, HIGH);
        digitalWrite(Motor0RW, HIGH);
        delay(500);
        digitalWrite(Motor0FW, LOW);
        digitalWrite(Motor0RW, LOW);
    }
    else if(motor == 1){
        digitalWrite(Motor1FW, HIGH);
        digitalWrite(Motor1RW, HIGH);
        delay(500);
        digitalWrite(Motor1FW, LOW);
        digitalWrite(Motor1RW, LOW);
    }
    else if(motor == 2){
        digitalWrite(Motor0FW, HIGH);
        digitalWrite(Motor0RW, HIGH);
        digitalWrite(Motor1FW, HIGH);
        digitalWrite(Motor1RW, HIGH);
        delay(500);
        digitalWrite(Motor0FW, LOW);
        digitalWrite(Motor0RW, LOW);
        digitalWrite(Motor1FW, LOW);
        digitalWrite(Motor1RW, LOW);
    }
}
void travaMotor(byte motor, int tempo){
    if(motor == 0){
        digitalWrite(Motor0FW, HIGH);
        digitalWrite(Motor0RW, HIGH);
        delay(tempo);
        digitalWrite(Motor0FW, LOW);
        digitalWrite(Motor0RW, LOW);
    }
    else if(motor == 1){
        digitalWrite(Motor1FW, HIGH);
        digitalWrite(Motor1RW, HIGH);
        delay(tempo);
        digitalWrite(Motor1FW, LOW);
        digitalWrite(Motor1RW, LOW);
    }
    else if(motor == 2){
        digitalWrite(Motor0FW, HIGH);
        digitalWrite(Motor0RW, HIGH);
        digitalWrite(Motor1FW, HIGH);
        digitalWrite(Motor1RW, HIGH);
        delay(tempo);
        digitalWrite(Motor0FW, LOW);
        digitalWrite(Motor0RW, LOW);
        digitalWrite(Motor1FW, LOW);
        digitalWrite(Motor1RW, LOW);
    }
}